using BepuPhysics.Collidables;
using BepuUtilities;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    public struct TrianglePairTester : IPairTester<TriangleWide, TriangleWide, Convex4ContactManifoldWide>
    {
        public int BatchSize => 32;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void GetIntervalForNormal(in Vector3Wide a, in Vector3Wide b, in Vector3Wide c, in Vector3Wide normal, out Vector<float> min, out Vector<float> max)
        {
            Vector3Wide.Dot(normal, a, out var dA);
            Vector3Wide.Dot(normal, b, out var dB);
            Vector3Wide.Dot(normal, c, out var dC);
            min = Vector.Min(dA, Vector.Min(dB, dC));
            max = Vector.Max(dA, Vector.Max(dB, dC));
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void GetDepthForNormal(in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB,
            in Vector3Wide normal, out Vector<float> depth)
        {
            GetIntervalForNormal(aA, bA, cA, normal, out var minA, out var maxA);
            GetIntervalForNormal(aB, bB, cB, normal, out var minB, out var maxB);
            depth = Vector.Min(maxA - minB, maxB - minA);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void TestEdgeEdge(
            in Vector3Wide edgeDirectionA, in Vector3Wide edgeDirectionB,
            in Vector3Wide aA, in Vector3Wide bA, in Vector3Wide cA, in Vector3Wide aB, in Vector3Wide bB, in Vector3Wide cB,
            out Vector<float> depth, out Vector3Wide normal)
        {
            // 校准法线,使其在规格化时从三角形指向长方体。
            Vector3Wide.CrossWithoutOverlap(edgeDirectionA, edgeDirectionB, out normal);
            Vector3Wide.Length(normal, out var normalLength);
            // 请注意,我们还没有校准。深度计算不依赖于校准,所以我们在测试完所有法线之后才开始计算。
            Vector3Wide.Scale(normal, Vector<float>.One / normalLength, out normal);
            GetDepthForNormal(aA, bA, cA, aB, bB, cB, normal, out depth);
            // 保护自己不受不良常态的影响。
            depth = Vector.ConditionalSelect(Vector.LessThan(normalLength, new Vector<float>(1e-10f)), new Vector<float>(float.MaxValue), depth);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void Select(
            ref Vector<float> depth, ref Vector3Wide normal,
            in Vector<float> depthCandidate, in Vector3Wide normalCandidate)
        {
            var useCandidate = Vector.LessThan(depthCandidate, depth);
            Vector3Wide.ConditionalSelect(useCandidate, normalCandidate, normal, out normal);
            depth = Vector.Min(depth, depthCandidate);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void Select(
            ref Vector<float> depth, ref Vector3Wide normal,
            in Vector<float> depthCandidate, in Vector<float> nxCandidate, in Vector<float> nyCandidate, in Vector<float> nzCandidate)
        {
            var useCandidate = Vector.LessThan(depthCandidate, depth);
            normal.X = Vector.ConditionalSelect(useCandidate, nxCandidate, normal.X);
            normal.Y = Vector.ConditionalSelect(useCandidate, nyCandidate, normal.Y);
            normal.Z = Vector.ConditionalSelect(useCandidate, nzCandidate, normal.Z);
            depth = Vector.Min(depth, depthCandidate);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void TryAddTriangleAVertex(in Vector3Wide vertex, in Vector<int> vertexId,
            in Vector3Wide tangentBX, in Vector3Wide tangentBY, in Vector3Wide triangleCenterB, in Vector3Wide contactNormal, in Vector3Wide faceNormalB,
            in Vector3Wide edgeABPlaneNormalB, in Vector3Wide edgeBCPlaneNormalB, in Vector3Wide edgeCAPlaneNormalB, in Vector3Wide bA, in Vector3Wide bB,
            in Vector<int> allowContacts, in Vector<float> inverseContactNormalDotFaceNormalB,
            ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount)
        {
            // 测试B的所有三条边的边缘平面符号。我们可以直接测试顶点而不是未投影的顶点,因为光线投射遵循接触法线,
            // 所有这些平面法线都垂直于接触法线。
            Vector3Wide.Subtract(vertex, bA, out var bAToVertex);
            Vector3Wide.Subtract(vertex, bB, out var bBToVertex);
            Vector3Wide.Dot(bAToVertex, edgeABPlaneNormalB, out var abDot);
            Vector3Wide.Dot(bBToVertex, edgeBCPlaneNormalB, out var bcDot);
            Vector3Wide.Dot(bAToVertex, edgeCAPlaneNormalB, out var caDot);
            var abContained = Vector.GreaterThan(abDot, Vector<float>.Zero);
            var bcContained = Vector.GreaterThan(bcDot, Vector<float>.Zero);
            var caContained = Vector.GreaterThan(caDot, Vector<float>.Zero);
            var contained = Vector.BitwiseAnd(abContained, Vector.BitwiseAnd(bcContained, caContained));

            // 将光线从三角形A的顶点沿接触法线投射到三角形B的平面,并检查是否包含。
            // 我们使用接触法线而不是面法线来减少接触生成对对排序的依赖。
            // (还有其他方法可以实现组合光线投射->容器测试;我们只使用简单的方法。)
            // (-contactNormal*t-vertexOnA+triangleCenterB)*faceNormalB=0
            // (-contactNormal*t+(triangleCenterB-vertexOnA)*faceNormalB=0
            // (-contactNormal*faceNormalB)*t=(triangleCenterB-vertexOnA)*faceNormalB
            // t=(vertexOnA-triangleCenterB)*faceNormalB/(contactNormal*faceNormalB)
            Vector3Wide.Subtract(triangleCenterB, vertex, out var offset);
            Vector3Wide.Dot(offset, faceNormalB, out var distance);
            var t = distance * inverseContactNormalDotFaceNormalB;
            Vector3Wide.Scale(contactNormal, t, out var unprojectedVertex);
            Vector3Wide.Add(unprojectedVertex, vertex, out unprojectedVertex);

            ManifoldCandidate candidate = default;
            Vector3Wide.Subtract(unprojectedVertex, triangleCenterB, out var offsetOnB);
            Vector3Wide.Dot(offsetOnB, tangentBX, out candidate.X);
            Vector3Wide.Dot(offsetOnB, tangentBY, out candidate.Y);
            candidate.FeatureId = vertexId;
            ManifoldCandidateHelper.AddCandidate(ref candidates, ref candidateCount, candidate, Vector.BitwiseAnd(allowContacts, contained), pairCount);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ClipEdge(in Vector3Wide edgeStart, in Vector3Wide edgeDirection, in Vector3Wide pointOnPlane, in Vector3Wide planeNormal, out Vector<float> entry, out Vector<float> exit)
        {
            // 边平面法线指向边界三角形的内侧。
            // 交点=点(planeNormal,pointOnPlane-edgeStart)/点(planeNormal,edgeDirectionB)
            Vector3Wide.Subtract(pointOnPlane, edgeStart, out var edgeToPlane);
            Vector3Wide.Dot(edgeToPlane, planeNormal, out var edgePlaneNormalDot);
            Vector3Wide.Dot(edgeDirection, planeNormal, out var velocity);
            var t = edgePlaneNormalDot / velocity;
            var isEntry = Vector.GreaterThanOrEqual(velocity, Vector<float>.Zero);
            var validVelocity = Vector.GreaterThan(Vector.Abs(velocity), new Vector<float>(1e-10f));
            entry = Vector.ConditionalSelect(Vector.BitwiseAnd(validVelocity, isEntry), t, new Vector<float>(float.MinValue));
            exit = Vector.ConditionalSelect(Vector.AndNot(validVelocity, isEntry), t, new Vector<float>(float.MaxValue));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ClipBEdgeAgainstABounds(
             in Vector3Wide edgeABPlaneNormalA, in Vector3Wide edgeBCPlaneNormalA, in Vector3Wide edgeCAPlaneNormalA,
             in Vector3Wide aA, in Vector3Wide aB,
             in Vector3Wide edgeDirectionB, in Vector3Wide edgeStartB, in Vector<int> entryId, in Vector<int> exitIdOffset,
             in Vector3Wide triangleCenterB, in Vector3Wide tangentBX, in Vector3Wide tangentBY,
             in Vector<float> epsilon, in Vector<int> allowContacts, ref ManifoldCandidate candidates, ref Vector<int> candidateCount, int pairCount)
        {
            // 基础id是沿着负boxEdgeDirection和boxEdgeCenterOffsetDirection的角点的ID。
            // edgeDirectionId是沿boxEdgeDirection移动到另一个顶点时要添加的量。
            // edgeCenterOffsetId是沿boxEdgeCenterOffsetDirection移动到另一个顶点时要添加的量。

            // 我们有三个由三角形A的边创建的边平面。
            // 我们要针对所有三条边测试三角形B边。
            ClipEdge(edgeStartB, edgeDirectionB, aA, edgeABPlaneNormalA, out var entryAB, out var exitAB);
            ClipEdge(edgeStartB, edgeDirectionB, aB, edgeBCPlaneNormalA, out var entryBC, out var exitBC);
            ClipEdge(edgeStartB, edgeDirectionB, aA, edgeCAPlaneNormalA, out var entryCA, out var exitCA);
            var entry = Vector.Max(Vector.Max(Vector<float>.Zero, entryAB), Vector.Max(entryBC, entryCA));
            var exit = Vector.Min(Vector.Min(Vector<float>.One, exitAB), Vector.Min(exitBC, exitCA));

            // entryX=点(entry*edgeDirectionA+edgeStartA-triangleCenterB,TangentBX)
            // entryY=点(entry*edgeDirectionA+edgeStartA-triangleCenterB,TangentBY)
            // exitX=点(exit*edgeDirectionA+edgeStartA-triangleCenterB,TangentBX)
            // 退出Y=点(退出*edgeDirectionA+edgeStartA-triangleCenterB,TangentBY)
            Vector3Wide.Subtract(edgeStartB, triangleCenterB, out var offset);
            Vector3Wide.Dot(offset, tangentBX, out var offsetX);
            Vector3Wide.Dot(offset, tangentBY, out var offsetY);
            Vector3Wide.Dot(tangentBX, edgeDirectionB, out var edgeDirectionX);
            Vector3Wide.Dot(tangentBY, edgeDirectionB, out var edgeDirectionY);

            ManifoldCandidate candidate = default;
            var six = new Vector<int>(6);
            // 条目
            var exists = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd(
                Vector.BitwiseAnd(
                    Vector.LessThan(candidateCount, six),
                    Vector.GreaterThanOrEqual(exit - entry, epsilon)),
                Vector.BitwiseAnd(
                    Vector.LessThan(entry, Vector<float>.One),
                    Vector.GreaterThan(entry, Vector<float>.Zero))));
            candidate.X = entry * edgeDirectionX + offsetX;
            candidate.Y = entry * edgeDirectionY + offsetY;
            candidate.FeatureId = entryId;
            ManifoldCandidateHelper.AddCandidate(ref candidates, ref candidateCount, candidate, exists, pairCount);
            // 出口
            exists = Vector.BitwiseAnd(allowContacts, Vector.BitwiseAnd(
                Vector.BitwiseAnd(
                    Vector.LessThan(candidateCount, six),
                    Vector.GreaterThanOrEqual(exit, entry)),
                Vector.BitwiseAnd(
                    Vector.LessThanOrEqual(exit, Vector<float>.One),
                    Vector.GreaterThanOrEqual(exit, Vector<float>.Zero))));
            candidate.X = exit * edgeDirectionX + offsetX;
            candidate.Y = exit * edgeDirectionY + offsetY;
            candidate.FeatureId = entryId + exitIdOffset;
            ManifoldCandidateHelper.AddCandidate(ref candidates, ref candidateCount, candidate, exists, pairCount);
        }



        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void Test(
            ref TriangleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin,
            ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
            out Convex4ContactManifoldWide manifold)
        {
            manifold = default;
            Matrix3x3Wide.CreateFromQuaternion(orientationA, out var worldRA);
            Matrix3x3Wide.CreateFromQuaternion(orientationB, out var worldRB);
            Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(worldRB, worldRA, out var rB);
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, worldRA, out var localOffsetB);
            Matrix3x3Wide.TransformWithoutOverlap(b.A, rB, out var bA);
            Vector3Wide.Add(bA, localOffsetB, out bA);
            Matrix3x3Wide.TransformWithoutOverlap(b.B, rB, out var bB);
            Vector3Wide.Add(bB, localOffsetB, out bB);
            Matrix3x3Wide.TransformWithoutOverlap(b.C, rB, out var bC);
            Vector3Wide.Add(bC, localOffsetB, out bC);

            Vector3Wide.Add(bA, bB, out var localTriangleCenterB);
            Vector3Wide.Add(localTriangleCenterB, bC, out localTriangleCenterB);
            Vector3Wide.Scale(localTriangleCenterB, new Vector<float>(1f / 3f), out localTriangleCenterB);

            Vector3Wide.Subtract(bB, bA, out var abB);
            Vector3Wide.Subtract(bC, bB, out var bcB);
            Vector3Wide.Subtract(bA, bC, out var caB);

            Vector3Wide.Add(a.A, a.B, out var localTriangleCenterA);
            Vector3Wide.Add(localTriangleCenterA, a.C, out localTriangleCenterA);
            Vector3Wide.Scale(localTriangleCenterA, new Vector<float>(1f / 3f), out localTriangleCenterA);

            Vector3Wide.Subtract(a.B, a.A, out var abA);
            Vector3Wide.Subtract(a.C, a.B, out var bcA);
            Vector3Wide.Subtract(a.A, a.C, out var caA);

            // A AB x*
            TestEdgeEdge(abA, abB, a.A, a.B, a.C, bA, bB, bC, out var depth, out var localNormal);
            TestEdgeEdge(abA, bcB, a.A, a.B, a.C, bA, bB, bC, out var depthCandidate, out var localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);
            TestEdgeEdge(abA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);

            // 公元前x*
            TestEdgeEdge(bcA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);
            TestEdgeEdge(bcA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);
            TestEdgeEdge(bcA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);

            // A CA x*
            TestEdgeEdge(caA, abB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);
            TestEdgeEdge(caA, bcB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);
            TestEdgeEdge(caA, caB, a.A, a.B, a.C, bA, bB, bC, out depthCandidate, out localNormalCandidate);
            Select(ref depth, ref localNormal, depthCandidate, localNormalCandidate);

            // 面法线
            Vector3Wide.CrossWithoutOverlap(abA, caA, out var faceNormalA);
            Vector3Wide.Length(faceNormalA, out var faceNormalALength);
            Vector3Wide.Scale(faceNormalA, Vector<float>.One / faceNormalALength, out faceNormalA);
            GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalA, out depthCandidate);
            Select(ref depth, ref localNormal, depthCandidate, faceNormalA);
            Vector3Wide.CrossWithoutOverlap(abB, caB, out var faceNormalB);
            Vector3Wide.Length(faceNormalB, out var faceNormalBLength);
            Vector3Wide.Scale(faceNormalB, Vector<float>.One / faceNormalBLength, out faceNormalB);
            GetDepthForNormal(a.A, a.B, a.C, bA, bB, bC, faceNormalB, out var faceDepthB);
            Select(ref depth, ref localNormal, faceDepthB, faceNormalB);

            // 按照惯例将法线从B指向A。
            Vector3Wide.Subtract(localTriangleCenterB, localTriangleCenterA, out var centerAToCenterB);
            Vector3Wide.Dot(localNormal, centerAToCenterB, out var calibrationDot);
            var shouldFlip = Vector.GreaterThan(calibrationDot, Vector<float>.Zero);
            localNormal.X = Vector.ConditionalSelect(shouldFlip, -localNormal.X, localNormal.X);
            localNormal.Y = Vector.ConditionalSelect(shouldFlip, -localNormal.Y, localNormal.Y);
            localNormal.Z = Vector.ConditionalSelect(shouldFlip, -localNormal.Z, localNormal.Z);

            Vector3Wide.Dot(localNormal, faceNormalA, out var localNormalDotFaceNormalA);
            Vector3Wide.Dot(localNormal, faceNormalB, out var localNormalDotFaceNormalB);
            // 我们将避免为法线离碰撞法线非常接近90度的三角形生成接触。
            // 这有助于避免一些数值问题,尽管它确实引入了一个怪现象-1e-4f的dotThreshold对应于在距离面法线最后一个度数左右的范围内(大致)没有生成碰撞。
            // 角度小于此角度的边根本不会生成接触。
            const float dotThreshold = 1e-4f;
            var allowContacts = Vector.BitwiseAnd(Vector.LessThan(localNormalDotFaceNormalA, new Vector<float>(-dotThreshold)), Vector.GreaterThan(localNormalDotFaceNormalB, new Vector<float>(dotThreshold)));
            if (Vector.EqualsAll(allowContacts, Vector<int>.Zero))
            {
                manifold.Contact0Exists = default;
                manifold.Contact1Exists = default;
                manifold.Contact2Exists = default;
                manifold.Contact3Exists = default;
                return;
            }

            // 此时,我们已经计算了最小深度和相关的局部法线。
            // 我们现在需要计算一些接触位置、它们的每次接触深度和功能ID。

            // 接触生成始终设定面-面剪裁。其他形式的联系人生成只是面对面的特例,而且由于我们支付
            // 对于所有代码路径,单独处理它们是没有意义的。

            // 我们将在三角形的曲面上工作,但我们仍然希望曲面的二维参数化以减少接触。
            // 因此,我们将从边和边x法线创建切线轴。
            Vector3Wide.LengthSquared(abB, out var abBLengthSquared);
            Vector3Wide.Scale(abB, Vector<float>.One / Vector.SquareRoot(abBLengthSquared), out var tangentBX);
            Vector3Wide.CrossWithoutOverlap(tangentBX, faceNormalB, out var tangentBY);

            // 请注意,我们最多只分配6名候选人。每条三角形边最多可以贡献两个接触(如果更多,则需要非凸形剪裁区域)。
            // 数字问题可能会导致更多的问题发生,但它们是被防范的(无论是直接的,还是在添加超过第六个候选人之前检查计数的意义上都是如此)。
            int byteCount = Unsafe.SizeOf<ManifoldCandidate>() * 6;
            var buffer = stackalloc byte[byteCount];
            var candidateCount = Vector<int>.Zero;
            ref var candidates = ref Unsafe.As<byte, ManifoldCandidate>(ref *buffer);

            // 虽然边剪裁将找到任何边-边或aVertex-bFace接触,但它不会找到bVertex-aFace接触。
            // 单独添加它们。
            // (首先添加这些允许我们简单地跳过容量测试,因为总共只能有三个bVertex-Aface触点。)
            // 请注意,被零除是受allowContacts保护的。
            var inverseContactNormalDotFaceNormalB = Vector<float>.One / localNormalDotFaceNormalB;
            Vector3Wide.CrossWithoutOverlap(abB, localNormal, out var edgeABPlaneNormalB);
            Vector3Wide.CrossWithoutOverlap(bcB, localNormal, out var edgeBCPlaneNormalB);
            Vector3Wide.CrossWithoutOverlap(caB, localNormal, out var edgeCAPlaneNormalB);
            TryAddTriangleAVertex(a.A, Vector<int>.Zero, tangentBX, tangentBY, localTriangleCenterB, localNormal, faceNormalB, edgeABPlaneNormalB, edgeBCPlaneNormalB, edgeCAPlaneNormalB, bA, bB, allowContacts, inverseContactNormalDotFaceNormalB, ref candidates, ref candidateCount, pairCount);
            TryAddTriangleAVertex(a.B, Vector<int>.One, tangentBX, tangentBY, localTriangleCenterB, localNormal, faceNormalB, edgeABPlaneNormalB, edgeBCPlaneNormalB, edgeCAPlaneNormalB, bA, bB, allowContacts, inverseContactNormalDotFaceNormalB, ref candidates, ref candidateCount, pairCount);
            TryAddTriangleAVertex(a.C, new Vector<int>(2), tangentBX, tangentBY, localTriangleCenterB, localNormal, faceNormalB, edgeABPlaneNormalB, edgeBCPlaneNormalB, edgeCAPlaneNormalB, bA, bB, allowContacts, inverseContactNormalDotFaceNormalB, ref candidates, ref candidateCount, pairCount);

            // 请注意,边情况还将添加三角形B边界内的三角形A顶点,因此不需要A顶点情况。
            // 请注意,每个呼叫可以生成4个联系人,因此我们必须开始检查容量。

            // 根据所涉及形状的大小创建用于比较的区分比例的epsilon。这有助于避免根据所涉及对象的大小而不同的行为。
            Vector3Wide.LengthSquared(abA, out var abALengthSquared);
            Vector3Wide.LengthSquared(caA, out var caALengthSquared);
            Vector3Wide.LengthSquared(caB, out var caBLengthSquared);
            var epsilonScale = Vector.SquareRoot(Vector.Min(
                Vector.Max(abALengthSquared, caALengthSquared),
                Vector.Max(abBLengthSquared, caBLengthSquared)));
            var edgeEpsilon = new Vector<float>(1e-5f) * epsilonScale;
            var exitIdOffset = new Vector<int>(3);
            // 请注意这里使用的是localNormal,而不是faceNormalA。为什么？就像顶点A的情况一样,我们不会在三角形A的面Voronoi区域中创建接触。
            // 相反,测试区域沿接触法线倾斜。这些平面与A的边相交,并使接触法线作为切线。
            // 这避免了对配对顺序的依赖(考虑A和B交换时会发生什么情况),并在拐角情况下产生更好的接触(考虑两个接近垂直的三角形)。
            Vector3Wide.CrossWithoutOverlap(localNormal, abA, out var edgeABPlaneNormalA);
            Vector3Wide.CrossWithoutOverlap(localNormal, bcA, out var edgeBCPlaneNormalA);
            Vector3Wide.CrossWithoutOverlap(localNormal, caA, out var edgeCAPlaneNormalA);
            ClipBEdgeAgainstABounds(edgeABPlaneNormalA, edgeBCPlaneNormalA, edgeCAPlaneNormalA, a.A, a.B, abB, bA, new Vector<int>(3), exitIdOffset, localTriangleCenterB, tangentBX, tangentBY, edgeEpsilon, allowContacts, ref candidates, ref candidateCount, pairCount);
            ClipBEdgeAgainstABounds(edgeABPlaneNormalA, edgeBCPlaneNormalA, edgeCAPlaneNormalA, a.A, a.B, bcB, bB, new Vector<int>(4), exitIdOffset, localTriangleCenterB, tangentBX, tangentBY, edgeEpsilon, allowContacts, ref candidates, ref candidateCount, pairCount);
            ClipBEdgeAgainstABounds(edgeABPlaneNormalA, edgeBCPlaneNormalA, edgeCAPlaneNormalA, a.A, a.B, caB, bC, new Vector<int>(5), exitIdOffset, localTriangleCenterB, tangentBX, tangentBY, edgeEpsilon, allowContacts, ref candidates, ref candidateCount, pairCount);

            Vector3Wide.Subtract(localTriangleCenterA, localTriangleCenterB, out var faceCenterBToFaceCenterA);
            ManifoldCandidateHelper.Reduce(ref candidates, candidateCount, 6, faceNormalA, localNormal, faceCenterBToFaceCenterA, tangentBX, tangentBY, epsilonScale, -speculativeMargin, pairCount,
                out var contact0, out var contact1, out var contact2, out var contact3,
                out manifold.Contact0Exists, out manifold.Contact1Exists, out manifold.Contact2Exists, out manifold.Contact3Exists);

            // 将触点转换为歧管。
            // 将基础移动到世界旋转中,这样我们就不需要变换单独的联系人了。
            Matrix3x3Wide.TransformWithoutOverlap(tangentBX, worldRA, out var worldTangentBX);
            Matrix3x3Wide.TransformWithoutOverlap(tangentBY, worldRA, out var worldTangentBY);
            Matrix3x3Wide.TransformWithoutOverlap(localTriangleCenterB, worldRA, out var worldTriangleCenter);
            Matrix3x3Wide.TransformWithoutOverlap(localNormal, worldRA, out manifold.Normal);
            manifold.Contact0Exists = Vector.BitwiseAnd(manifold.Contact0Exists, allowContacts);
            manifold.Contact1Exists = Vector.BitwiseAnd(manifold.Contact1Exists, allowContacts);
            manifold.Contact2Exists = Vector.BitwiseAnd(manifold.Contact2Exists, allowContacts);
            manifold.Contact3Exists = Vector.BitwiseAnd(manifold.Contact3Exists, allowContacts);
            TransformContactToManifold(contact0, worldTriangleCenter, worldTangentBX, worldTangentBY, out manifold.OffsetA0, out manifold.Depth0, out manifold.FeatureId0);
            TransformContactToManifold(contact1, worldTriangleCenter, worldTangentBX, worldTangentBY, out manifold.OffsetA1, out manifold.Depth1, out manifold.FeatureId1);
            TransformContactToManifold(contact2, worldTriangleCenter, worldTangentBX, worldTangentBY, out manifold.OffsetA2, out manifold.Depth2, out manifold.FeatureId2);
            TransformContactToManifold(contact3, worldTriangleCenter, worldTangentBX, worldTangentBY, out manifold.OffsetA3, out manifold.Depth3, out manifold.FeatureId3);
            // 请注意,我们优先考虑三角形B。边界平滑仅在两个网格中的一个网格上执行。
            var faceFlag = Vector.ConditionalSelect(
                Vector.GreaterThanOrEqual(localNormalDotFaceNormalB, new Vector<float>(MeshReduction.MinimumDotForFaceCollision)), new Vector<int>(MeshReduction.FaceCollisionFlag), Vector<int>.Zero);
            manifold.FeatureId0 += faceFlag;
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void TransformContactToManifold(
            in ManifoldCandidate rawContact, in Vector3Wide faceCenterB, in Vector3Wide tangentBX, in Vector3Wide tangentBY,
            out Vector3Wide manifoldOffsetA, out Vector<float> manifoldDepth, out Vector<int> manifoldFeatureId)
        {
            Vector3Wide.Scale(tangentBX, rawContact.X, out manifoldOffsetA);
            Vector3Wide.Scale(tangentBY, rawContact.Y, out var y);
            Vector3Wide.Add(manifoldOffsetA, y, out manifoldOffsetA);
            Vector3Wide.Add(manifoldOffsetA, faceCenterB, out manifoldOffsetA);
            manifoldDepth = rawContact.Depth;
            manifoldFeatureId = rawContact.FeatureId;
        }

        public void Test(ref TriangleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }

        public void Test(ref TriangleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }
    }
}
